#include "bsp_can.h"
#include "main.h"

extern CAN_HandleTypeDef hcan1;
extern CAN_HandleTypeDef hcan2;


CAN_TxHeaderTypeDef  can1_tx_message,can2_tx_message;
uint8_t 						 can1_tx_data[8],can2_tx_data[8];


void can_filter_init(void)
{

	CAN_FilterTypeDef can_filter_st;
	can_filter_st.FilterActivation = ENABLE;
	can_filter_st.FilterMode = CAN_FILTERMODE_IDMASK;
	can_filter_st.FilterScale = CAN_FILTERSCALE_32BIT;
	can_filter_st.FilterIdHigh = 0x0000;
	can_filter_st.FilterIdLow = 0x0000;
	can_filter_st.FilterMaskIdHigh = 0x0000;
	can_filter_st.FilterMaskIdLow = 0x0000;
	can_filter_st.FilterBank = 0;
	can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO0;
	HAL_CAN_ConfigFilter(&hcan1, &can_filter_st);
	HAL_CAN_Start(&hcan1);
	HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);


	can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO1;
	can_filter_st.SlaveStartFilterBank = 14;
	can_filter_st.FilterBank = 14;
	HAL_CAN_ConfigFilter(&hcan2, &can_filter_st);
	HAL_CAN_Start(&hcan2);
	HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING);
}



void can1_send_message( uint32_t std_id,int16_t motor1,int16_t motor2,int16_t motor3,int16_t motor4 )
{
	uint32_t send_mail_box;
	
	if( std_id != 0x200 && std_id != 0x1FF )
	{
		return;
	}
	
	can1_tx_message.StdId = std_id;
	can1_tx_message.IDE = CAN_ID_STD;
	can1_tx_message.RTR = CAN_RTR_DATA;
	can1_tx_message.DLC = 0x08;
	
	can1_tx_data[0] = motor1 >> 8;
	can1_tx_data[1] = motor1;
	can1_tx_data[2] = motor2 >> 8;
	can1_tx_data[3] = motor2;
	can1_tx_data[4] = motor3 >> 8;
	can1_tx_data[5] = motor3;
	can1_tx_data[6] = motor4 >> 8;
	can1_tx_data[7] = motor4;
	
	
	HAL_CAN_AddTxMessage(&hcan1, &can1_tx_message, can1_tx_data, &send_mail_box);
}


void can2_send_message( uint32_t std_id,int16_t motor1,int16_t motor2,int16_t motor3,int16_t motor4 )
{
	uint32_t send_mail_box;
	
	if( std_id != 0x200 && std_id != 0x1FF )
	{
		return;
	}
	
	can2_tx_message.StdId = std_id;
	can2_tx_message.IDE = CAN_ID_STD;
	can2_tx_message.RTR = CAN_RTR_DATA;
	can2_tx_message.DLC = 0x08;
	
	can2_tx_data[0] = motor1 >> 8;
	can2_tx_data[1] = motor1;
	can2_tx_data[2] = motor2 >> 8;
	can2_tx_data[3] = motor2;
	can2_tx_data[4] = motor3 >> 8;
	can2_tx_data[5] = motor3;
	can2_tx_data[6] = motor4 >> 8;
	can2_tx_data[7] = motor4;
	
	HAL_CAN_AddTxMessage(&hcan2, &can2_tx_message, can2_tx_data, &send_mail_box);
}


void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
	CAN_RxHeaderTypeDef can_header;
	uint8_t rx_data[8];
	
	HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &can_header, rx_data);
	
	if( can_header.StdId > 0x200 && can_header.StdId < 0x209)
	{	
		c610_can_recv_callback(can_header.StdId,rx_data);
	}
}


void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
	CAN_RxHeaderTypeDef can_header;
	uint8_t rx_data[8];
	
	HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO1, &can_header, rx_data);
	
	if( can_header.StdId > 0x200 && can_header.StdId < 0x209)
	{	
		c610_can_recv_callback(can_header.StdId,rx_data);
	}
}



